#include "Common.h"
#include "PhysicsCommon.h"
#include "CollisionSystem.h"
#include "CollisionSkin.h"
#include "PhysicsBody.h"
#include "PhysicsPrimitive.h"
#include <algorithm>
#include <limits>

using namespace Physics;

CollisionSystem::CollisionSystem()
{
}

CollisionSystem::~CollisionSystem()
{
}

CollisionSystemSAP::CollisionSystemSAP()
	: mDirty(false)
	, mLagestX(0)
{
}

CollisionSystemSAP::~CollisionSystemSAP()
{
}

void CollisionSystemSAP::AddSkin(CollisionSkin* skin)
{
	mDirty = true;

	skin->setCollisionSystem(this);
	
	const AxisAlignedBox& worldBounds = skin->getWorldBoundingBox();
	if (worldBounds.Valid() && mLagestX < worldBounds.getSize().X)
		mLagestX = worldBounds.getSize().X;

	mSkins.push_back(skin);
}

bool CollisionSystemSAP::RemoveSkin(CollisionSkin* skin)
{
	SkinVector::iterator it = std::find(mSkins.begin(), mSkins.end(), skin);
	if (it == mSkins.end())
		return false;

	if (*it != mSkins.back())
		*it = mSkins.back();
	mSkins.pop_back();
	return true;
}

void CollisionSystemSAP::Extract(const Vector3& min, const Vector3& max, SkinVector& skins)
{
	if (mSkins.empty())
		return;

	sort();

	size_t i = bsearch(min.X - mLagestX);

	float xMax = max.X;
	while (i < mSkins.size() && mSkins[i]->getWorldBoundingBox().getMin().X < xMax)
	{
		if (mSkins[i]->getWorldBoundingBox().getMax().X > min.X)
			skins.push_back(mSkins[i]);
		++i;
	}
}

void CollisionSystemSAP::sort()
{
	if (!mDirty)
		return;
	
	struct Compare {
		bool operator() (CollisionSkin* A, CollisionSkin* B) {
			return A->getWorldBoundingBox().getMin().X <
				B->getWorldBoundingBox().getMin().X;
		}
	} SkinCompare;

	std::sort(mSkins.begin(), mSkins.end(), SkinCompare);
	mDirty = false;
}

size_t CollisionSystemSAP::bsearch(float x)
{
	//  It is up to the caller to make sure this isn't called on an empty collection.
	size_t top = mSkins.size();
	size_t bot = 0;
	while (top > bot)
	{
		size_t mid = (top + bot) >> 1;
		if (mSkins[mid]->getWorldBoundingBox().getMin().X >= x)
			top = mid;
		else
			bot = mid + 1;
	}
	return top;
}

bool CollisionSystemSAP::SegmentIntersect(
	float& outFrac, 
	CollisionSkin*& outSkin, 
	Vector3& outPos, 
	Vector3& outNormal, 
	const Segment& seg)
{
	outFrac = std::numeric_limits<float>::max();
	outSkin = NULL;
	outPos = outNormal = Vector3::Zero;

	AxisAlignedBox box;
	box.Merge(seg.Origin);
	box.Merge(seg.GetEnd());

	float frac;
	Vector3 pos;
	Vector3 normal;

	mActiveSkins.clear();
	Extract(box.getMin(), box.getMax(), mActiveSkins);

	for (SkinVector::iterator it(mActiveSkins.begin()), end(mActiveSkins.end()); it != end; ++it)
	{
		CollisionSkin* skin = *it;
		if (!box.OverlapTest(skin->getWorldBoundingBox()))
			continue;

		if (!skin->SegmentIntersect(frac, pos, normal, seg))
			continue;

		if (frac >= 0 && frac < outFrac)
		{
			outFrac = frac;
			outSkin = skin;
			outPos = pos;
			outNormal = normal;
		}
	}
	return (outFrac <= 1.0f);
}


bool CollisionSystemSAP::SphereIntersect(
	float& outFrac, 
	CollisionSkin*& outSkin, 
	Vector3& outPos, 
	Vector3& outNormal, 
	const struct Segment& seg,
	float radius)
{
	outFrac = std::numeric_limits<float>::max();
	outSkin = NULL;
	outPos = outNormal = Vector3::Zero;

	AxisAlignedBox box;
	box.Merge(seg.Origin);
	box.Merge(seg.GetEnd());
	box.Merge(box.getMin() - Vector3(radius * 2));
	box.Merge(box.getMax() + Vector3(radius * 2));

	float frac;
	Vector3 pos;
	Vector3 normal;

	mActiveSkins.clear();
	Extract(box.getMin(), box.getMax(), mActiveSkins);

	for (SkinVector::iterator it(mActiveSkins.begin()), end(mActiveSkins.end()); it != end; ++it)
	{
		CollisionSkin* skin = *it;
		if (!box.OverlapTest(skin->getWorldBoundingBox()))
			continue;

		if (!skin->SphereIntersect(frac, pos, normal, seg, radius))
			continue;

		if (frac >= 0 && frac < outFrac)
		{
			outFrac = frac;
			outSkin = skin;
			outPos = pos;
			outNormal = normal;
		}
	}
	return (outFrac <= 1.0f);
}
